#ifndef FHE_MATH_VEC_H
#define FHE_MATH_VEC_H

#include <fhe/math/Math.h>
#include <fhe/core/FileSystem.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <typeinfo>

namespace fhe
{
  namespace core
  {
    template <typename T>
    class Vec<2,T>
    {
    public:
      typedef Rot<2,T> R;
      
      T x;
      T y;
      
      Vec( T _x, T _y )
        : x( _x )
        , y( _y )
      {
      }
      
      Vec( const R& rot, T radius = 1 )
        : x( radius * Math::cos( rot.radians() ) )
        , y( radius * Math::sin( rot.radians() ) )
      {
      }
      
      Vec()
        : x( 0 )
        , y( 0 )
      {
      }
      
      T operator[]( size_t i ) const
      {
        switch ( i )
        {
          case 0: return x;
          case 1: return y;
          default: FHE_ERROR( "invalid Vec2 index %d", i );
        }
      }
      
      T& operator[]( size_t i )
      {
        switch ( i )
        {
          case 0: return x;
          case 1: return y;
          default: FHE_ERROR( "invalid Vec2 index %d", i );
        }
      }
      
      bool operator==( const Vec& v ) const
      {
        return Math::equal( x, v.x ) && Math::equal( y, v.y );
      }
      
      bool operator!=( const Vec& v ) const
      {
        return !operator==(v);
      }
      
      T hash() const
      {
        T hash = 23;
        hash = hash * 31 + x;
        hash = hash * 31 + y;
        return hash;
      }
      
      bool operator<( const Vec& v ) const
      {
        return hash() < v.hash();
      }
      
      Vec operator-() const
      {
        return Vec( -x, -y );
      }
      
      Vec operator+( const Vec& v ) const
      {
        return Vec( x + v.x, y + v.y );
      }

      Vec operator-( const Vec& v ) const
      {
        return Vec( x - v.x, y - v.y );
      }

      Vec operator*( const Vec& v ) const
      {
        return Vec( x * v.x, y * v.y );
      }

      Vec operator/( const Vec& v ) const
      {
        return Vec( x / v.x, y / v.y );
      }

      Vec operator*( double d ) const
      {
        return Vec( x * d, y * d );
      }

      Vec operator/( double d ) const
      {
        return Vec( x / d, y / d );
      }
      
      Vec operator*( const R& r ) const
      {
        return Vec( toRot() + r, length() );
      }
      
      void operator+=( const Vec& v )
      {
        x += v.x;
        y += v.y;
      }

      void operator-=( const Vec& v )
      {
        x -= v.x;
        y -= v.y;
      }

      void operator*=( const Vec& v )
      {
        x *= v.x;
        y *= v.y;
      }

      void operator/=( const Vec& v )
      {
        x /= v.x;
        y /= v.y;
      }

      void operator*=( double d )
      {
        x *= d;
        y *= d;
      }

      void operator/=( double d )
      {
        x /= d;
        y /= d;
      }
      
      T squaredLength() const
      {
        return x * x + y * y;
      }
      
      T length() const
      {
        return Math::sqrt( squaredLength() );
      }
      
      R toRot() const
      {
        return R::fromRadians( Math::atan2( y, x ) );
      }
      
      Vec normalize() const
      {
        return operator/( length() );
      }
      
      T dot( const Vec& v ) const
      {
        return x * v.x + y * v.y;
      }
      
      Vec project( const Vec& v ) const
      {
        return v * ( dot( v ) / v.squaredLength() );
      }
      
      T cross( const Vec& v ) const
      {
        return x * v.y - y * v.x;
      }
      
      Vec perp() const
      {
        return Vec( -y, x );
      }
      
      T distToLine( const Vec& a, const Vec& b ) const
      {
        return operator-( a ).cross( b - a ) / ( b - a ).length();
      }
      
      static std::string typeName()
      {
        return std::string( "Vec2" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        std::ostringstream os;
        os << typeName() << "(" << x << ", " << y << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Vec v )
      {
        FileSystem::instance().serialize( data->addChild( "x" ), v.x );
        FileSystem::instance().serialize( data->addChild( "y" ), v.y );
      }
      
      static Vec deserialize( const Data::Ptr& data )
      {
        return Vec( data->is<T>( "x" ) ? data->get<T>( "x" ) : 0,
                    data->is<T>( "y" ) ? data->get<T>( "y" ) : 0 );
      }
      
      static const Vec ZERO;
      static const Vec ONE;
    };
    
    template <typename T>
    const Vec<2,T> Vec<2,T>::ZERO( 0, 0 );

    template <typename T>
    const Vec<2,T> Vec<2,T>::ONE( 1, 1 );
    
    template <typename T>
    class Vec<3,T>
    {
    public:
      typedef Rot<3,T> R;
      
      T x;
      T y;
      T z;
      
      Vec()
        : x( 0 )
        , y( 0 )
        , z( 0 )
      {
      }
      
      Vec( T _x, T _y, T _z )
        : x( _x )
        , y( _y )
        , z( _z )
      {
      }
      
      T operator[]( size_t i ) const
      {
        switch( i )
        {
          case 0: return x;
          case 1: return y;
          case 2: return z;
          default: FHE_ERROR( "invalid vec3 index" ); throw;
        }
      }
      
      T& operator[]( size_t i )
      {
        switch( i )
        {
          case 0: return x;
          case 1: return y;
          case 2: return z;
          default: FHE_ERROR( "invalid vec3 index" ); throw;
        }
      }
      
      bool operator==( const Vec& v ) const
      {
        return Math::equal( x, v.x ) && Math::equal( y, v.y ) && Math::equal( z, v.z );
      }
      
      bool operator!=( const Vec& v ) const
      {
        return !operator==( v );
      }
      
      Vec operator-() const
      {
        return Vec( -x, -y, -z );
      }
      
      Vec operator+( const Vec& v ) const
      {
        return Vec( x + v.x, y + v.y, z + v.z );
      }
      
      Vec operator-( const Vec& v ) const
      {
        return Vec( x - v.x, y - v.y, z - v.z );
      }
      
      Vec operator*( const Vec& v ) const
      {
        return Vec( x * v.x, y * v.y, z * v.z );
      }
      
      Vec operator/( const Vec& v ) const
      {
        return Vec( x / v.x, y / v.y, z / v.z );
      }
      
      Vec operator*( T d ) const
      {
        return Vec( x * d, y * d, z * d );
      }
      
      Vec operator/( T d ) const
      {
        return Vec( x / d, y / d, z / d );
      }
      
      Vec operator*( const R& r ) const
      {
        return r * *this;
      }
      
      void operator+=( const Vec& v )
      {
        x += v.x;
        y += v.y;
        z += v.z;
      }
      
      void operator-=( const Vec& v )
      {
        x -= v.x;
        y -= v.y;
        z -= v.z;
      }
      
      void operator*=( const Vec& v )
      {
        x *= v.x;
        y *= v.y;
        z *= v.z;
      }
      
      void operator/=( const Vec& v )
      {
        x /= v.x;
        y /= v.y;
        z /= v.z;
      }
      
      void operator*=( T d )
      {
        x *= d;
        y *= d;
        z *= d;
      }
      
      void operator/=( T d )
      {
        x /= d;
        y /= d;
        z /= d;
      }
      
      T squaredLength() const
      {
        return x * x + y * y + z * z;
      }
      
      T length() const
      {
        return Math::sqrt( squaredLength() );
      }
      
      Vec normalize() const
      {
        return operator/( length() );
      }
      
      T dot( const Vec& v ) const
      {
        return x * v.x + y * v.y + z * v.z;
      }
      
      Vec cross( const Vec& v ) const
      {
        return Vec( y * v.z - z * v.y,
                    z * v.x - x * v.z,
                    x * v.y - y * v.x );
      }
      
      Vec project( const Vec& v ) const
      {
        return v * ( dot( v ) / v.squaredLength() );
      }
      
      static std::string typeName()
      {
        return std::string( "Vec3" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        std::ostringstream os;
        os << typeName() << "(" << x << ", " << y << ", " << z << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Vec v )
      {
        FileSystem::instance().serialize( data->addChild( "x" ), v.x );
        FileSystem::instance().serialize( data->addChild( "y" ), v.y );
        FileSystem::instance().serialize( data->addChild( "z" ), v.z );
      }
      
      static Vec deserialize( const Data::Ptr& data )
      {
        return Vec( data->is<T>( "x" ) ? data->get<T>( "x" ) : 0,
                    data->is<T>( "y" ) ? data->get<T>( "y" ) : 0, 
                    data->is<T>( "z" ) ? data->get<T>( "z" ) : 0 );
      }
      
      static const Vec ZERO;
      static const Vec ONE;
    };
    
    template <typename T>
    const Vec<3,T> Vec<3,T>::ZERO( 0, 0, 0 );
    
    template <typename T>
    const Vec<3,T> Vec<3,T>::ONE( 1, 1, 1 );
    
    template <size_t D, typename T>
    std::ostream& operator<<( std::ostream& os, const Vec<D,T>& v )
    {
      return os << v.toString();
    }
    
    typedef Vec<2,double> Vec2d;
    typedef Vec<2,int> Vec2i;
    typedef Vec<3,double> Vec3d;
    typedef Vec<3,int> Vec3i;
  }
}

#endif
